#include <avr/io.h>
#include <avr/wdt.h>
#include "auvlib_usb.h"
#include "a2d6ch.h"
#include "motorpwm.h"




////////////////////////////////////////////////////////////////////
// ICD Stuff

u08 ucStatus = STAT_ERROR; // default; indicates BIT failed
            // STAT_IDLE               indicates BIT passed

// ordinarily, these would be defined as floats, and you may define them that way, if you wish.
u32 fVoltage = 0x40A00000; // 5 V.  This should be updated by an ADC ISR.
u32 fCurrent = 0x3E19999A; // 0.15 A.  This should be updated by an ADC ISR.

u08 g_bWasWatchDogReset;

////////////////////////////////////////////////////////////////////
// Motor-Board-Specific Stuff

u08 g_uchMotorAbsent = 0x3F;         // for now, assume none are present.  cached BIT result.
u16 g_ausCurZero[6]  = {490,490, 490,490, 490,490};  // midpoints for current sense, in bits. (not 512 due to Vdivide mistake)
s16 g_asBITresult[6] = {  0,  0,   0,  0,   0,  0};  // results of BIT current sense w/ PWM applied, in bits; 0 is open.
u16 g_usNumBITruns   = 0;            // how many times has BIT run (regardless of "success")?

s08 const ovcur_thresh      = 40; // 4 A.
s16 const ovcur_thresh_adc4 = 36; // 4 A (0.2759 dA/bit, nominal, but already divided by 4 (otherwise would be 145))


// Deadman stuff:
static volatile u08 stop = 0;
static volatile u08 mcRefresh = 0;
static          u08 DeadManCounter;
static          u08 DeadManTimeout;
static          u08 timerDividerM = 0;



// See comments in motorpwm.h.
s08 g_aschSpeedBank0[2] = {0,0};
s08 g_aschSpeedBank1[2] = {0,0};
s08 g_aschSpeedBank2[2] = {0,0};




// A 14MHz clock and 64 prescaler means this timeout should run 
// at 900 Hz (overflow on an 8bit timer)

// Divide this down to 112.5Hz (to make deadman timer more sane)
void DeadManTimer(void) {
  if( timerDividerM == 7 ) {
    // Count up the deadman timer
    if(++DeadManCounter > DeadManTimeout ) {
      setBlue(stop ? 0 : 1);
      stop = 1;
      mcRefresh = 1;
      DeadManCounter = 0;
    }
  }

  timerDividerM++;
  timerDividerM&=7;
}







////////////////////////////////////////////////////////////////////
// Main

int main() {
//int16_t i;

  // Disable watchdog, then check whether watchdog caused reset, then reset that flag.
  wdt_disable();
  g_bWasWatchDogReset = MCUSR & _BV(WDRF) ? 1 : 0;
  MCUSR = 0; 
  // ToDo: have a message that allows PC to clear this flag, which will then require it to be defined at file scope.

  // Inputs w/o pull-ups, to start:
  DDRA = 0; PORTA = 0;
  DDRB = 0; PORTB = 0;
  DDRC = 0; PORTC = 0;
  DDRD = 0; PORTD = 0;
  DDRE = 0; PORTE = 0;
  DDRF = 0; PORTF = 0;
  DDRG = 0; PORTG = 0;
  DDRH = 0; PORTH = 0;
  DDRJ = 0; PORTJ = 0;
  DDRK = 0; PORTK = 0;

  // This reconfigures pins:
  //   Should not touch PH0-1,PD0-1,PA4,PB0-3 but can reconfigure.
  //   Probably overrides function of PE1,PE0,PD2,PD3,PD5,PH0,PH1,PH2,PJ0,PJ1,PJ2.  PH2,PJ2 are ok as long as USART not in sync mode (UCSR*C)
  //   Should make PE2,PE1 outputs, PE0 input.  We should ignore.  (Note PE2 set in uart4.c)
  //   Should make PG0,PG1,PG2,PG5 outputs.  We should ignore.
  auvlibInit();
  sei();


  // Enable pull-ups on all N.C.'s:  (pulling up is currently disabled till we test w/o first.  it's just power-saving.)
  DDRA &= ~PORTA_PU; //PORTA |= PORTA_PU;
  DDRB &= ~PORTB_PU; //PORTB |= PORTB_PU;
  DDRC &= ~PORTC_PU; //PORTC |= PORTC_PU;
  DDRD &= ~PORTD_PU; //PORTD |= PORTD_PU;
  DDRE &= ~PORTE_PU; //PORTE |= PORTE_PU;
  DDRF &= ~PORTF_PU; //PORTF |= PORTF_PU;
  DDRG &= ~PORTG_PU; //PORTG |= PORTG_PU;
  DDRH &= ~PORTH_PU; //PORTH |= PORTH_PU;
  DDRJ &= ~PORTJ_PU; //PORTJ |= PORTJ_PU;
  DDRK &= ~PORTK_PU; //PORTK |= PORTK_PU;
  DDRL &= ~PORTL_PU; //PORTL |= PORTL_PU;

  // Enable pull-ups on presence pins.
  DDRA &= ~CUSTA_PU;   PORTA |= CUSTA_PU;
  DDRC &= ~CUSTC_PU;   PORTC |= CUSTC_PU;
  DDRJ &= ~CUSTJ_PU;   PORTJ |= CUSTJ_PU;

  // Enable inputs, where required (ADC, as well as non-existent PG6-7)
  DDRF &= ~CUSTF_IN;
  DDRG &= ~CUSTG_IN;
  DDRK &= ~CUSTK_IN;

  // Enable outputs, where required (FWD/REV/LED/SIWU)
  DDRA |=  CUSTA_OU; 
  DDRD |=  CUSTD_OU;
  DDRE |=  CUSTE_OU;
  DDRG |=  CUSTG_OU;
  DDRH |=  CUSTH_OU;
  DDRJ |=  CUSTJ_OU;
  DDRL |=  CUSTL_OU;
  // Outputs should already be zero after reset, but don't touch, in case auvlibInit() changed them.

  // Overridden outputs don't always have data direction overridden, particularly in the case of output compare.
  DDRB |=  CUSTB_OO;
  DDRE |=  CUSTE_OO; // this line is technically unnecessary, but for consistency.
  DDRH |=  CUSTH_OO;
  DDRL |=  CUSTL_OO;

  // Don't really need to do anything on overridden inputs (there currently is only one, RXD0).







  //////// TRUTH TABLE FOR H-BRIDGE STUFF
  //  REV  FWD  PWM  (assume REV/FWD active high)
  //   0    1    1   Forward
  //   1    0    1   Reverse
  //   1    1    X   connect both sides of motor to 28V
  //   0    0    X   disconnect motor
  //   0    1    0   disconnect motor
  //   1    0    0   disconnect motor

  //////// PROCEDURE FOR CHANGING DIRECTION
  // Set FWD,REV,PWM low.  Wait X ms.  Set FWD/REV, enable PWM.

  //////// PINOUT
  //
  // PORT A (secondary fn:  address/data):
  //   N.C.:  0,4-7
  //   PA1 :  3rd HB reverse (A).
  //   PA2 :  3rd HB present.  Active-low, needs internal pullup.
  //   PA3 :  3rd HB forward (A).
  // PORT B (secondary functions related to SPI, pc interrupts, timers):
  //   N.C.:  0(/SS),4(OC2A),7(OC0A,OC1C)
  //   ISP :  1,2,3
  //   PB5 :  2nd HB pwm     (B), OC1A
  //   PB6 :  2nd HB pwm     (A), OC1B
  // PORT C (secondary fn:  address):
  //   N.C.:  1-7
  //   PC0 :  1st HB present.  Active-low, needs internal pullup.
  // PORT D (secondary functions: I2C, USART, timers)
  //   N.C.:  0-5
  //   PD6 :  1st HB forward (B).
  //   PD7 :  1st HB reverse (B).
  // PORT E (secondary functions: timers, analog, int)
  //   N.C.:  3-7
  //   PE0 :  RXD0           (FT2232D ch B), input
  //   PE1 :  TXD0           (FT2232D ch B), output
  //   PE2 :  send-immediate (FT2232D ch B), output
  // PORT F (secondary functions: ADC, JTAG)
  //   N.C.:  N/A
  //   GND :  0-3 (CAREFUL!!!) -- set as input, no pull-up... (also ADC0-3)
  //   JTAG:  4-7 (also ADC4-7)
  // PORT G (secondary functions: memory strobes, timers)
  //   N.C.:  3,4 (also 6,7 do not exist)
  //   PG0 :  LED, red  .  Active-high.
  //   PG1 :  LED, green.  Active-high.
  //   PG2 :  LED, amber.  Active-high.
  //   PG5 :  LED, blue .  Active-high.
  // PORT H (secondary functions: USART, timers)
  //   N.C.:  0,1,6,7
  //   PH2 :  3rd HB forward (B).
  //   PH3 :  3rd HB reverse (B). (OC4A, irrelevant)
  //   PH4 :  3rd HB pwm     (B). (OC4B)
  //   PH5 :  3rd HB pwm     (A). (OC4C)
  // PORT J (secondary functions: USART, pc interrupts)
  //   N.C.:  0,1,7
  //   PJ2 :  2nd HB forward (A).
  //   PJ3 :  2nd HB reverse (A).
  //   PJ4 :  2nd HB present.  Active-low, needs internal pullup.
  //   PJ5 :  2nd HB forward (B).
  //   PJ6 :  2nd HB reverse (B).
  // PORT K (secondary functions: ADC, pc interrupts)
  //   N.C.:  N/A
  //   GND :  0-1 (CAREFUL!!!) -- set as input, no pull-up... (also ADC8-9)
  //   PK2 :  3rd HB current (A). (ADC10)
  //   PK3 :  3rd HB current (B). (ADC11)
  //   PK4 :  2nd HB current (A). (ADC12)
  //   PK5 :  2nd HB current (B). (ADC13)
  //   PK6 :  1st HB current (A). (ADC14)
  //   PK7 :  1st HB current (B). (ADC15)
  // PORT L (secondary functions: timers)
  //   N.C.:  0-2,5
  //   PL3 :  1st HB pwm     (A). (OC5A)
  //   PL4 :  1st HB pwm     (B). (OC5B)
  //   PL6 :  1st HB forward (A).
  //   PL7 :  1st HB reverse (A).








  DeadManCounter = 0;
  DeadManTimeout = 120; // 1.067 seconds.
  timer2SetPrescaler(TIMERRTC_CLK_DIV64);
  timerAttach( TIMER2OVERFLOW_INT, DeadManTimer );

  a2dStuffInit();

  // uncomment this when other stuff is verified to be working:
  a2dStartConvert(); // Needed for BIT.

  MotorInit(); // BIT inside here.
  MotorStop(); // Should be redundant, but be sure.

//  wdt_enable( 5 );


  if(g_uchMotorAbsent || BridgesAbsent())
    ucStatus = STAT_ERROR;
  else
    ucStatus = STAT_IDLE;


  while(1) {
    // If we receive any message, reset the dead man counter
    if( auvlibProcess() ) {
      DeadManCounter = 0;
    }

    // Motor refresh block, should run at around 50Hz (HTF do we know the rate???)
    if( mcRefresh ) {
      setRed(1);
      if( stop ) {
        g_aschSpeedBank0[0] = g_aschSpeedBank0[1] = 0;
        g_aschSpeedBank1[0] = g_aschSpeedBank1[1] = 0;
        g_aschSpeedBank2[0] = g_aschSpeedBank2[1] = 0;

        MotorStop   ();
      } else
        UpdateMotors();
      setRed(0);

      mcRefresh = 0;
    }
  }

  return 1;
}


// Standard error code from ICD.
//   If newmode == 1:  Takes adc values in motor order, divided by 4 (rounding mode not crucial here).
//   If newmode == 0:  Takes cur values in motor order, in deci-amps (rounding mode not crucial here).
static u16 computeError(s08 const current[6], u08 newmode) {
  u08 ret[2] = {0, 0}; // Start by assuming no error.

  if(BridgesAbsent()) {
    ret[0] = 1; // H-Bridge disconnection.
    ret[1] = BridgesAbsent();
  } else if(g_uchMotorAbsent) {
    ret[0] = 2; // Motor disconnection.
    ret[1] = g_uchMotorAbsent;
  } else {
    u08 ovc=0,i,j;
    if(newmode) {
      for(i=0,j=1; i<6; i++,j<<=1) {
        if(((s08)current[i]) > ovcur_thresh_adc4 || ((s08)current[i]) < -ovcur_thresh_adc4)
          ovc |= j;
      }
    } else {
      for(i=0,j=1; i<6; i++,j<<=1) {
        if(((s08)current[i]) > ovcur_thresh      || ((s08)current[i]) < -ovcur_thresh     )
          ovc |= j;
      }
    }

    if(ovc) {
      stop = mcRefresh = 1;

      ret[0] = 3; // Overcurrent.
      ret[1] = ovc;
    } else if(g_bWasWatchDogReset) {
      ret[0] = 4; // WDT reset occurred.
    } else if(0/*packet error; not currently reported, though*/) {
      ret[0] = 5; // Communication error.
    }
    // Else, ret remains 0.
  }

  return *(u16 *)ret;
}



////////////////////////////////////////////////////////////////////
// More ICD Stuff

// Returns size of payload, if TxIgnore not set.  Must return 0 if TxBusy||TxNotImpl||TxIgnore.
// TxBusy,TxNotImpl should not be set if TxIgnore is set.
u08 auvlibMsgHandler(u08 type, u08 len, u08 const *data) {
  u08 i;
  s16 tempaccum;
  TxIgnore=1;           // assume no ack wanted.
  TxBusy=TxNotImpl = 0; // assume neither is true.
  switch(type) {
    ////////////////////////////////////////// BASE MESSAGES
    case 0x01: // processor type
      if(len!=0)
        return 0;
      TxPacket[0]=0x20;
#if   defined(__AVR_AT90CAN128__)
      TxPacket[1]=0;
#elif defined(__AVR_ATmega2561__) || defined(__AVR_ATmega2560__)
      TxPacket[1]=1;
#elif defined(__AVR_ATmega644__ )
      TxPacket[1]=2;
#else
#error Unknown MCU
#endif
      TxIgnore = 0;
      return 2;    // 2 byte payload
    case 0x02: // get software version
      if(len!=0)
        return 0;
      *(u16 *)(TxPacket+0) = VERSION_MAJOR;
      *(u16 *)(TxPacket+2) = VERSION_MINOR;
      *(u16 *)(TxPacket+4) = VERSION_MICRO;
      TxIgnore = 0;
      return 6;    // 6 byte payload
    case 0x03: // processor description
      if(len!=0)
        return 0;
      for(i=0; szProcDesc[i]; i++)
        TxPacket[i]=szProcDesc[i]; // don't copy terminator.
      TxIgnore = 0;
      return i;    // strlen(szProcDesc) byte payload
    case 0x04: // max payload
      if(len!=0)
        return 0;
      TxPacket[0]=RX_PACKET_SIZE;
      TxPacket[1]=sizeof(TxPacket);
      TxIgnore = 0;
      return 2;    // 2 byte payload
    case 0x05: // board function
      if(len!=0)
        return 0;
      TxPacket[0]=0x02; // motor controller
      TxIgnore = 0;
      return 1;    // 1 byte payload
    case 0x06: // board serial number
      if(len!=0)
        return 0;
      TxPacket[0]=szBoardSerno[0];
      TxPacket[1]=szBoardSerno[1];
      TxPacket[2]=szBoardSerno[2];
      TxPacket[3]=szBoardSerno[3];
      TxPacket[4]=szBoardSerno[4];
      TxPacket[5]=szBoardSerno[5];
      TxIgnore = 0;
      return 6;    // 6 byte payload
    case 0x07: // board description
      if(len!=0)
        return 0;
      for(i=0; szBoardDesc[i]; i++)
        TxPacket[i]=szBoardDesc[i]; // don't copy terminator.
      TxIgnore = 0;
      return i;    // strlen(szBoardDesc) byte payload
    case 0x08: // board status
      if(len!=0)
        return 0;
      TxPacket[0] = ucStatus;
      TxIgnore = 0;
      return 1;    // 1 byte payload


    ////////////////////////////////////////// STANDARD MESSAGES
    case 0x10: // board voltage
      if(len!=0)
        return 0;
      *(u32 *)(TxPacket+0) = *(u32 *)&fVoltage;
      TxIgnore = 0;
      return 4;    // 4 byte payload
    case 0x11: // board current
      if(len!=0)
        return 0;
      *(u32 *)(TxPacket+0) = *(u32 *)&fCurrent;
      TxIgnore = 0;
      return 4;    // 4 byte payload
    case 0x12: // board reset (comms IC)
    case 0x13: // MCU   reset
      if(len!=0)
        return 0;
      TxNotImpl = 1;
      TxIgnore  = 0;
      return 0;    // 0 byte payload
    case 0x14: // status storage
      if(len!=1 || data[0]>0x02)
        return 0;
      TxNotImpl = 1;
      TxIgnore  = 0;
      return 0;    // 0 byte payload


    ////////////////////////////////////////// BOARD-SPECIFIC MESSAGES


    case 0x20: // set motor thrust and direction (deprecated version; see 0x23!)
      if(len!=8)
        return 0;
      g_aschSpeedBank0[0] = data[0];
      g_aschSpeedBank0[1] = data[1];
      g_aschSpeedBank1[0] = data[2];
      g_aschSpeedBank1[1] = data[3];
      g_aschSpeedBank2[0] = data[4];
      g_aschSpeedBank2[1] = data[5];
      // data[6], data[7] are ignored.  This corresponded to the
      //   ill-fated fourth channel of the MSP-based logic boards.

      // ToDo: consider clamping those to sane values to avoid killing motors.

      if( stop )
        stop = 0;

      mcRefresh = 1;

      // The DC gain for the current sense is:
      //   *) nominally 185 mV/A at the ACS712-x05B, according to the datasheet
      //   *) 220/230 (unitless) at the buffer
      //   *) 1024 bits / 5V at the ADC
      //   *) we want the answer in deci-amps.
      //   *) the zero point is 512 bits (nominally)
      // The zero-point can and should be calibrated by the BIT.
      // From room temp (25 C) to operating temp (assume 55 C), the zero moves about -2.4 mV, or 0.5 LSbit.       (datasheet)
      // From room temp (25 C) to operating temp (assume 55 C), the sensitivity moves about -0.24 mV/A, or 0.13%. (datasheet)
      // The 2k bleeder total will also indicate a current of up to 0.5 LSbit, due to 14mA at 28V.
      //
      // 10 dA | 1     A | 230 | 5    V
      // ------+---------+-----+----------- approx = 0.2759 dA/bit .
      // 1   A | 0.185 V | 220 | 1024 bits
      //
      // A suitable approximation is round(round((inval-zero)*3/8)*47/64), to 16 bits.
      // This is no less accurate than doing *141/512 (about +/- 0.8 deciamps), and, 
      //    with proper clamping, maxes out at +/- 9024, instead of 72192, allowing s16 instead of s24.
      // With a perfect zero, we still maintain about 90% of the input range (+/- 12.75A instead of +/- 14.1A)
      // Quantities outside +/- 5A are not necessarily accurate on the ACS chip anyway.
      //
      // Any further scale factor adjustment away from data sheet values will happen on PC side.
      //
      // If I could settle for a scale factor of 181.5 mV/A (which is within the possible range of 180-190),
      //   then the even simpler formula of round((inval-zero)*9/32) would also work, maxing out at 4608.

      // Note that the expected 0 is 490, not 512, because the divider goes to 0, not to 2.5V.

      for(i=0; i<6; i++) {
////    tempaccum = (s16)(g_nA2DData[g_Mot2adc[i]] - 490                       ); // remove nominal   zero point.
        tempaccum = (s16)(g_nA2DData[g_Mot2adc[i]] - g_ausCurZero[g_Mot2adc[i]]); // remove estimated zero point.
        //if(tempaccum < -511)      // This clamp is unnecessary, since the difference
        //  tempaccum  = -511;      //   shouldn't exceed +/- 1024, even with a weird zero.
        //if(tempaccum >  511)      //   This means the +/- 18048 must be handled as an 
        //  tempaccum  =  511;      //   intermediate, which fits in s16.

#if       1
        // Even on -O0, compiler is smart enough not to do an actual divison, and
        //   apparently shifts don't work right with signed numbers (well, rounding
        //   idiocy, anyway).  Apparently HW mult is only 2 cycles.
        tempaccum *= 3;
        tempaccum /= 8;
        tempaccum *= 47;
        tempaccum /= 64;
#else  // 1
        // Ok, it was still rounding stupidly (rounding towards zero, instead of towards -inf now), and 
        //   I'm tired of testing this.  The above block mistakenly caused it to increase negative values 
        //   by about 0.1A, and decrease positive values by about 0.1A.

        tempaccum *=  3;
        tempaccum +=  4;
        tempaccum >>= 3; // note:  towards -inf
        tempaccum *= 47;
        tempaccum += 32;
        tempaccum >>= 6; // note:  towards -inf
#endif // 1

        if(tempaccum < -128)
          tempaccum  = -128; // Eh, screw the ICD.  I bet this is faster than clamping to -127.
        if(tempaccum >  127)
          tempaccum  =  127;
        TxPacket[i] = (u08)(tempaccum&0xFF); // There's potentially a faster way to write this... (ToDo)
      }

      *(u16 *)(TxPacket+6) = 0; // fourth channel
      *(u16 *)(TxPacket+8) = computeError((s08 *)(TxPacket+0), 0); // cast back to signed.

      // Format is 6 bytes of current sense, in fixed-point decimal format (e.g., 101 means +10.1A)
      //           2 bytes of zeros (corresponded to non-existent fourth channel)
      //           2 bytes of error codes



      TxIgnore = 0;
      return 10; // 10 byte payload
    case 0x21: // get motor status (deprecated, and probably won't be replaced.  This was only kept to support thermistors.)
      if(len!=0)
        return 0;

      // Ignore deadman.  0x20 is required to re-enable motors.  All other
      //   messages are keepalive, but don't "revive", to continue a bad analogy.

      // Assumption here is that PC never sent -128.  If it did, it won't (shouldn't?) 
      //   go to motors, but it'll get returned anyway.
      TxPacket[0] = g_aschSpeedBank0[0]; // Current PWM.  Spec says -127 to 127.
      TxPacket[1] = g_aschSpeedBank0[1];
      TxPacket[2] = g_aschSpeedBank1[0];
      TxPacket[3] = g_aschSpeedBank1[1];
      TxPacket[4] = g_aschSpeedBank2[0];
      TxPacket[5] = g_aschSpeedBank2[1];

      *(u16 *)(TxPacket+6)=0; // non-existent fourth channel.
      *(u32 *)(TxPacket+8)=0; // non-existent thermistors.

      TxIgnore = 0;
      return 12; // 12 byte payload
    case 0x22: // perform built-in tests (deprecated version; see 0x24,0x25!)
      if(len!=0)
        return 0;

      RunBIT();
      mcRefresh = 1; // As a side-effect, BIT changed the values.  Fix, if necessary.  Don't touch "stop".
      // Could add a "g_needs_blink" value, and check it in the main loop.  Just don't want to delay msg response.  (ToDo)

      // Format is:  first byte is H-bridge board presence:  0x01 drive, 0x02 vert, 0x04 horz
      //             second byte is motor presence        :  0x03 drive, 0x0C vert, 0x30 horz (in obvious way)

      TxPacket[0] = BridgesAbsent()  ^ 7;
      TxPacket[1] = g_uchMotorAbsent ^ 0x3F;

      TxIgnore = 0;
      return 2; // 2 byte payload.
    case 0x23: // set motor thrust and direction (preferred over 0x20!)
      if(len!=6)
        return 0;
      g_aschSpeedBank0[0] = data[0];
      g_aschSpeedBank0[1] = data[1];
      g_aschSpeedBank1[0] = data[2];
      g_aschSpeedBank1[1] = data[3];
      g_aschSpeedBank2[0] = data[4];
      g_aschSpeedBank2[1] = data[5];

      // ToDo: consider clamping those to sane values to avoid killing motors.

      if( stop )
        stop = 0;

      mcRefresh = 1;

      TxPacket[6] = 0; // This holds the second-LSbits of the conversions.
                       //   Presently the actual LSbits are discarded.

      for(i=0; i<6; i++) {
////    tempaccum = (s16)(g_nA2DData[g_Mot2adc[i]] - 490                       ); // remove nominal   zero point.
        tempaccum = (s16)(g_nA2DData[g_Mot2adc[i]] - g_ausCurZero[g_Mot2adc[i]]); // remove estimated zero point.

        if     (tempaccum < -511)
          tempaccum       = -511;
        else if(tempaccum > 511)
          tempaccum       =  511;

        // N.B.:  / rounds towards zero, >> rounds towards -inf.

        tempaccum   /= 2; // we will not be passing the LSbit back, so eliminate it.

        TxPacket[6] |= (tempaccum&1) << i; // Stuff second-LSbit.

        tempaccum  >>= 1; // already passed that bit back.
        TxPacket[i]  = (u08)(tempaccum&0xFF); // Save high 8-MSbits.
      }

      *(u16 *)(TxPacket+7) = computeError((s08 *)(TxPacket+0), 1); // cast back to signed

      // Format is 6 bytes of current sense, 8 MSbits of 10-bit ADC value.
      //           1 byte containing 2nd-LSbit of 10-bit ADC value.  LSbit is discarded.
      //           2 bytes of error codes

      TxIgnore = 0;
      return 9; // 9 byte payload
    case 0x24: // request new BIT (not fully implemented yet!) (preferred over 0x22!)
      if(len!=0)
        return 0;

      RunBIT(); // This should really be deferred to the main loop to give us more time!
      mcRefresh = 1; // As a side-effect, BIT changed the values.  Fix, if necessary.  Don't touch "stop".
      // Could add a "g_needs_blink" value, and check it in the main loop.  Just don't want to delay msg response.  (ToDo)

      TxIgnore = 0;
      return 0; // 0 byte payload.
    case 0x25: // Request BIT results (preferred over 0x22!)
      if(len!=0)
        return 0;

      // Format is:  first byte is H-bridge board presence:  0x01 drive, 0x02 vert, 0x04 horz
      //             second byte is motor presence        :  0x03 drive, 0x0C vert, 0x30 horz (in obvious way)

      TxPacket[0] = BridgesAbsent()  ^ 7;
      TxPacket[1] = g_uchMotorAbsent ^ 0x3F;
      *(u16 *)(TxPacket + 2) = g_usNumBITruns; // how many BITs have been run?

      for(i=0; i<6; i++) // estimated zero-point, 12 bytes.
        *(u16 *)(TxPacket + i*2 +  4) = g_ausCurZero [g_Mot2adc[i]];

      for(i=0; i<6; i++) // measured Isense in BIT, with zero-point subtracted, 12 bytes.
        *(s16 *)(TxPacket + i*2 + 16) = g_asBITresult[          i ];

      TxIgnore = 0;
      return 28; // 28 byte payload.
    default: // unknown
      return 0;
  }
}
